#include "App/gyro_algorithm.h"
#include "App/line_algorithm.h"
#include "App/mv_serial_data_parse.h"
#include "App/task.h"
#include "Components/ATK_MS901M/atk_ms901m.h"
#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include "Components/simple_json.h"
#include "oled_ui.h"
#include <math.h>

#define TAG "Main"

edc_task_type_t edc_task_type = EDC_TASK_NONE;
bool is_cur_task_running = false;

int target = 2;

void taskRst() {
  switch ((int)edc_task_type) {
  case EDC_TASK_1:
    edc_task1_init();
    break;
  case EDC_TASK_3:
    edc_task3_init();
    break;
  case EDC_TASK_2:
    edc_task2_init();
    break;
  default:
    break;
  }

  is_cur_task_running = true;
  log(TAG, "set task to %d", (int)edc_task_type);
}

void handleSerial() {
  if (serial_is_available()) {
    char *str = serial_read_string();
    log(TAG, "serial_rec:%s", str);
    if (strstr(str, "start") != NULL) {
      is_cur_task_running = true;
      log(TAG, "start task");
      taskRst();
    } else if (strstr(str, "stop") != NULL) {
      is_cur_task_running = false;
      log(TAG, "stop task");
    }
    if (mjson_get_int(str, "target", &target) != -1)
      edc_task1_set_target_circle(target);

    int task_idx = 0;
    if (mjson_get_int(str, "task", &task_idx) != -1) {
      edc_task_type = (edc_task_type_t)task_idx;
      taskRst();
      log(TAG, "set task to %d", task_idx);
    }
    int target_angle = 0;
    if (mjson_get_int(str, "user_target_angle", &target_angle) != -1) {
      edc_task3_set_target_angle(target_angle);
      log(TAG, "set user_target_angle to %d", target_angle);
    }
  }
}
void stopTask() {
  log(TAG, "stop task");
  is_cur_task_running = false;
  edc_task_type = EDC_TASK_NONE;

  motor_set_mode(MOTOR_RIGHT, MOTOR_NEUTRAL);
  motor_set_mode(MOTOR_LEFT, MOTOR_NEUTRAL);
}
void startTrackMode(void) {
  edc_task_type = EDC_TASK_1;
  taskRst();
  uiSetModeStatus(0, 1);
}

void stopTrackMode(void) {
  stopTask();
  uiSetModeStatus(0, 0); // 更新UI状态为已停止
}

void startFixedLightingMode(void) {
  edc_task_type = EDC_TASK_2;
  taskRst();
  uiSetModeStatus(1, 1);
}

void stopFixedLightingMode(void) {
  stopTask();
  uiSetModeStatus(1, 0);
}

void startFreeLightingMode(void) {
  edc_task_type = EDC_TASK_3;
  taskRst();
  uiSetModeStatus(2, 1);
}

void stopFreeLightingMode(void) {
  stopTask();
  uiSetModeStatus(2, 0);
}
void registUIModeCallbacks(void) {
  uiRegisterStartCallback(0, startTrackMode); // 循迹模式
  uiRegisterStopCallback(0, stopTrackMode);

  uiRegisterStartCallback(1, startFixedLightingMode); // 固定L模式
  uiRegisterStopCallback(1, stopFixedLightingMode);

  uiRegisterStartCallback(2, startFreeLightingMode); // 自由L模式
  uiRegisterStopCallback(2, stopFreeLightingMode);
}

int main(void) {
  SYSCFG_DL_init();
  SysTick_Init();
  uart_init();
  uiInterfaceInit();
  registUIModeCallbacks();
  atk_ms901m_init();
  motion_init();

  log(TAG, "Main task Initialize Done!!!");

  OLED_ShowString(0, 0, "Hello Electronics Design Contest test SUCESS!!!3", 16);

  delay(500); // wait for sensor init done

  edc_task_type = EDC_TASK_3;
  taskRst();

  uiInterfaceUpdate();

  for (;;) {
    handleSerial();
    if (uiShouldCheckKey()) {
      uiHandleInput(uiGetKeyCode());
      uiInterfaceUpdate();
    }

    if (is_cur_task_running) {
      switch ((int)edc_task_type) {
      case EDC_TASK_1:
        edc_task1_pool();
        break;
      case EDC_TASK_3:
        edc_task3_pool();
        break;
      case EDC_TASK_2:
        edc_task2_pool();
        break;
      }
    }
  }
}
